#include "MeasurE616AlgSideSe10Edge.h"

MeasurE616AlgSideSE10Edge::MeasurE616AlgSideSE10Edge(int i):MeasurE616AlgBase(i)
{

}

void MeasurE616AlgSideSE10Edge::operator()(cv::Mat &srcImg, int id,std::map<string, float> &measRes,
                                           int fixtureId)
{

//side 图    SE10(FAI37) paras616\side\SE.xml
    //    int LDmeasure(cv::Mat srcimg, const vector<int> paras, vector<cv::Point2f>& respts);

    std::vector<std::vector<cv::Point2f>> resptsVec;
    resptsVec.resize(2);
    {
        std::vector<int> tparas;
        int i=40;
        for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
        {
            tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(0,j));
        }
        std::vector<cv::Rect> rangVec;
        TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
        cv::Mat imgt=srcImg(rangVec[0]).clone();
        std::vector<cv::Point2f> respts;
        MeasurE616EdgeDetect obj;

        obj.LDmeasurerevise(imgt, tparas, respts);

        ImageCoordinateTransformation(respts, rangVec);

        std::unique_lock<std::mutex> lock(*MeasurE616AlgBase::mtxptrsideVec[id]);
        while (!MeasurE616AlgBase::flagsideVec[id])
        {
            (*MeasurE616AlgBase::cvarptrsideVec[id]).wait(lock);
        }

        for(auto &elem:respts)
        {
            cv::Point2f ptsOut;
            CoordinateTransformationRelateToDot(datumSideVec[id], elem, ptsOut);
            resptsVec[0].push_back(ptsOut);
        }
    }

    {
        std::vector<int> tparas;
        int i=40;
        for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
        {
            tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(1,j));
        }
        std::vector<cv::Rect> rangVec;
        TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
        cv::Mat imgt=srcImg(rangVec[0]).clone();
        std::vector<cv::Point2f> respts;
        MeasurE616EdgeDetect obj;
        obj.LDmeasurerevise(imgt,tparas,respts);

        ImageCoordinateTransformation(respts,rangVec);

        std::unique_lock<std::mutex> lock(*MeasurE616AlgBase::mtxptrsideVec[id]);
        while (!MeasurE616AlgBase::flagsideVec[id])
        {
            (*MeasurE616AlgBase::cvarptrsideVec[id]).wait(lock);
        }

        for(auto &elem:respts)
        {
            cv::Point2f ptsOut;
            CoordinateTransformationRelateToDot(datumSideVec[id], elem, ptsOut);
            resptsVec[1].push_back(ptsOut);
        }
    }

//    for(int i=0;i<contours.size();++i){
//        cv::circle(srcimg,contours[i],1,160,-1);
//    }

    std::string prefix = m_xmlFileNamePre + to_string(fixtureId) + "_";
    measRes.insert(std::make_pair(string("FAI36-SE10-X1-distance_pixel"), resptsVec[0][0].y));
    float fai36se10x1 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI36-SE10-X1-distance")], resptsVec[0][0].y);
    measRes.insert(std::make_pair(string("FAI36-SE10-X1-distance_real"), fai36se10x1));

    measRes.insert(std::make_pair(string("FAI36-SE10-X2-distance_pixel"), resptsVec[0][1].y));
    float fai36se10x2 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI36-SE10-X2-distance")], resptsVec[0][1].y);
    measRes.insert(std::make_pair(string("FAI36-SE10-X2-distance_real"), fai36se10x2));

    measRes.insert(std::make_pair(string("FAI36-SE10-X3-distance_pixel"), resptsVec[0][2].y));
    float fai36se10x3 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI36-SE10-X3-distance")], resptsVec[0][2].y);
    measRes.insert(std::make_pair(string("FAI36-SE10-X3-distance_real"), fai36se10x3));

    measRes.insert(std::make_pair(string("FAI37-SE10-X1-distance_pixel"), resptsVec[1][0].y));
    float fai37se10x1 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI37-SE10-X1-distance")], resptsVec[1][0].y);
    measRes.insert(std::make_pair(string("FAI37-SE10-X1-distance_real"), fai37se10x1));

    measRes.insert(std::make_pair(string("FAI37-SE10-X2-distance_pixel"), resptsVec[1][2].y));
    float fai37se10x2 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI37-SE10-X2-distance")], resptsVec[1][2].y);
    measRes.insert(std::make_pair(string("FAI37-SE10-X2-distance_real"), fai37se10x2));

    measRes.insert(std::make_pair(string("FAI37-SE10-X3-distance_pixel"), resptsVec[1][3].y));
    float fai37se10x3 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI37-SE10-X3-distance")], resptsVec[1][3].y);
    measRes.insert(std::make_pair(string("FAI37-SE10-X3-distance_real"), fai37se10x3));

    measRes.insert(std::make_pair(string("FAI37-SE10-X4-distance_pixel"), resptsVec[1][5].y));
    float fai37se10x4 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI37-SE10-X4-distance")], resptsVec[1][5].y);
    measRes.insert(std::make_pair(string("FAI37-SE10-X4-distance_real"), fai37se10x4));

    measRes.insert(std::make_pair(string("FAI38-SE10-X1-distance_pixel"), fabs(resptsVec[1][0].y-resptsVec[1][1].y)));
    float fai38se10x1 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI38-SE10-X1-distance")], fabs(resptsVec[1][0].y-resptsVec[1][1].y));
    measRes.insert(std::make_pair(string("FAI38-SE10-X1-distance_real"), fai38se10x1));

    measRes.insert(std::make_pair(string("FAI38-SE10-X2-distance_pixel"), fabs(resptsVec[1][1].y-resptsVec[1][2].y)));
    float fai38se10x2 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI38-SE10-X2-distance")], fabs(resptsVec[1][1].y-resptsVec[1][2].y));
    measRes.insert(std::make_pair(string("FAI38-SE10-X2-distance_real"), fai38se10x2));

    measRes.insert(std::make_pair(string("FAI38-SE10-X3-distance_pixel"), fabs(resptsVec[1][3].y-resptsVec[1][4].y)));
    float fai38se10x3 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI38-SE10-X3-distance")], fabs(resptsVec[1][3].y-resptsVec[1][4].y));
    measRes.insert(std::make_pair(string("FAI38-SE10-X3-distance_real"), fai38se10x3));

    measRes.insert(std::make_pair(string("FAI38-SE10-X4-distance_pixel"), fabs(resptsVec[1][4].y-resptsVec[1][5].y)));
    float fai38se10x4 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI38-SE10-X4-distance")], fabs(resptsVec[1][4].y-resptsVec[1][5].y));
    measRes.insert(std::make_pair(string("FAI38-SE10-X4-distance_real"), fai38se10x4));
}
